#include "navigation.h"

Navigation::Navigation(std::shared_ptr<Params>& params_): params(params_)
{
    //定义nh
    ros::NodeHandle nh;
    pub_goal_ = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
}


/**
 * @brief 发布地图上的目标位置
 * @param (x, y) - 坐标
 * 		  z-角度
 * @return
 * */
void Navigation::PublishGoalCmd(float x, float y, float z)
{
    geometry_msgs::PoseStamped goal;
    goal.header.frame_id = "map";
    goal.pose.position.x = x;
    goal.pose.position.y = y;
    goal.pose.orientation.z = sin(z / 2);
    goal.pose.orientation.w = cos(z / 2);
    pub_goal_.publish(goal);
}

/**
 * @brief 发布需要执行的速度到采集板
 * @param v-线速度
 * 		  w-角速度
 * @return
 * */
void Navigation::PublishMotorCmd(float v, float w)
{
    geometry_msgs::Twist speed;
    speed.linear.x = v;
    speed.angular.z = w;
    cmd_vel_.publish(speed);
}

// 根据目标点和自身位置，调节acion的线速度和角速度,直到到达目标点，误差范围为0.01m
bool Navigation::DockingTaskAction_WithPCL(ACTION_DATA_S* action, const Point2D &g_pose_2d, const Point2D &g_cross_point_) 
{
    // 目标点：params->g_cross_point_:x,y,yaw
    // 自身位置：params->g_carto_x_, params->g_carto_y_, params->g_carto_yaw_
    float angular_speed = 0.15;
    // 角速度处理
    double delta_angle = NormalizeAngle(g_pose_2d.yaw - g_cross_point_.yaw);
    if (std::abs(delta_angle) > TOLERANCE) {
        action->w = delta_angle > 0 ? -angular_speed : angular_speed;
    }
    else {
        action->w = 0;
    }

    float linear_speed = 0.15;
    // 线速度处理
    double delta_x = g_cross_point_.x - g_pose_2d.x;
    double delta_y = g_cross_point_.y - g_pose_2d.y;
    double delta_distance = std::sqrt(delta_x * delta_x + delta_y * delta_y);

    if (delta_distance > 0.01) {
        action->v = linear_speed;
    }
    else {
        action->v = 0;
    }

    return (std::abs(delta_angle) <= TOLERANCE && delta_distance <= 0.01);
}

/**
 * @brief 规范化角度到 [-PI, PI] 范围内
 * @param angle 输入角度
 * @return 规范化后的角度
 */
double Navigation::NormalizeAngle(double angle)
{
    while (angle > M_PI) angle -= 2 * M_PI;
    while (angle < -M_PI) angle += 2 * M_PI;
    return angle;
}